#include "cartographer/rcm/submap/submap_update.h"
#include "cartographer/mapping/2d/probability_grid_range_data_inserter_2d.h"

namespace cartographer
{
    using namespace mapping;

    bool CompareClosureInfoScore(ClosureInfo a, ClosureInfo b)
    {
        return a.score < b.score;
    }

    // sensor::RangeData TransformCloudToRange(const sensor::PointCloud &cloud, const Eigen::Vector3f &origin)
    // {
    //     sensor::RangeData range;
    //     range.returns = cloud;
    //     range.origin = origin;
    //     for (auto &point : cloud.points())
    //     {
    //         sensor::RangefinderPoint miss;
    //         miss.position = origin_in_local + options_.missing_data_ray_length() / range * delta;
    //         range.misses.push_back(hit);
    //     }
    //     range.origin;
    // }

    SubmapUpdator2d::SubmapUpdator2d()
    {
    }

    void SubmapUpdator2d::AddRangeDataInserter(int trajectory_id, const proto::SubmapsOptions2D &options)
    {
        if (range_inserters_.find(trajectory_id) != range_inserters_.end())
            return;
        // 参考submap_2d中的函数CreateRangeDataInserter
        // std::unique_ptr<RangeDataInserterInterface>
        CHECK_EQ(options.range_data_inserter_options().range_data_inserter_type(),
                 proto::RangeDataInserterOptions::PROBABILITY_GRID_INSERTER_2D);
        auto range_inserter = std::make_shared<ProbabilityGridRangeDataInserter2D>(
            options.range_data_inserter_options().probability_grid_range_data_inserter_options_2d());
        range_inserters_[trajectory_id] = range_inserter;
    }

    void SubmapUpdator2d::UpdateClosureRelations(const std::vector<Constraint> &result)
    {
        int interSize = 0;
        int addSize = 0;
        for (auto &cost : result)
        {
            if (cost.tag != Constraint::Tag::INTER_SUBMAP)
                continue;
            if (cost.node_id.trajectory_id == 0)
                continue;
            interSize++;
            if (UpdateClosureRelations(cost))
                addSize++;
        }
        LOG(INFO) << "UpdateClosureRelations interSize " << interSize << " addSize " << addSize;
    }

    bool SubmapUpdator2d::UpdateClosureRelations(const Constraint &cost)
    {
        if (!CheckClosureRelation(cost.submap_id, cost.node_id))
            return false;
        ClosureId closure_id(cost.submap_id, cost.node_id);
        auto now_time = std::chrono::steady_clock::now();
        bool needAdd = false;
        if (closureMapping_.find(cost.node_id) == closureMapping_.end())
        {
            needAdd = true;
            updateFlags_[cost.node_id] = false;
        }
        else
        {
            if (closureMapping_[cost.node_id].score < cost.score)
                needAdd = true;
        }
        if (needAdd)
        {
            closureMapping_[cost.node_id] = ClosureInfo{closure_id, cost.pose, now_time, cost.score};
            Print("ClosureIntraAdd", closureMapping_[cost.node_id]);
        }
        return needAdd;
    }

    std::vector<ClosureInfo> SubmapUpdator2d::GetNeedUpdateSubmaps()
    {
        std::vector<ClosureInfo> closures;
        for (auto &iter : updateFlags_)
        {
            const auto &nodeId = iter.first;
            if (iter.second || nodeId.trajectory_id == 0)
                continue;
            if (closureMapping_.find(nodeId) == closureMapping_.end())
                continue;
            closures.push_back(closureMapping_[nodeId]);
        }
        return closures;
    }

    bool SubmapUpdator2d::CheckClosureRelation(const SubmapId &submap_id, const NodeId &node_id)
    {
        int node_index = node_id.node_index;
        int node_trajectory_id = node_id.trajectory_id;
        int submap_index = submap_id.submap_index;
        int submap_trajectory_id = submap_id.trajectory_id;
        if (submap_trajectory_id == node_trajectory_id)
            return false;
        return true;
    }

    bool SubmapUpdator2d::InsertRange(Submap2D *submap, const ClosureId &closure_id, const sensor::RangeData &cloudGlobal2d)
    {
        const auto &node_id = closure_id.node_id;
        int node_traj_id = node_id.trajectory_id;
        int map_traj_id = closure_id.submap_id.trajectory_id;
        std::shared_ptr<mapping::RangeDataInserterInterface> inserter;
        if (range_inserters_.find(map_traj_id) == range_inserters_.end())
        {
            if (range_inserters_.find(node_traj_id) == range_inserters_.end())
                return false;
            inserter = range_inserters_[node_traj_id];
        }
        else
            inserter = range_inserters_[map_traj_id];
        inserter->Insert(cloudGlobal2d, submap->grid_.get());
        submap->set_num_range_data(submap->num_range_data() + 1);
        updateFlags_[node_id] = true;
        return true;
    }
} // namespace cartographer